/*
 * @brief: 反馈驱动
 * @date: 2015.09.06
 */

#include "fb.h"
#include "dispatch.h"
#include "act_types.h"
#include "cmd.h"
#include "feedback.h"
#include <assert.h>
#include <string.h>
#include <stdio.h>

/*
 * @param state[out]: 错误反馈
 * @param type: 0-左机头，1-右机头
 */
int fb_get_error_state(MH_UDP *pudp, int *state, int type)
{
    short query = type ? CMD_QUERY_FB_IN_CTEST_RC : CMD_QUERY_FB_IN_CTEST;
    QUERY_FB_IN_CTEST_RESP resp;
    int ret = 0;
    int i = 0;
    ERROR_BITS error_bits;
    memset(&resp, 0, sizeof(resp));

    for (i = 0; i < 2; i++)
    {
        ret = cmd_query(pudp, (uchar*)&query, sizeof (query), (RESPONSE*)&resp, sizeof (resp));
        if (ret == 0)
        {
            memcpy(&error_bits, &resp.cerror, sizeof(resp.cerror));
            error_bits.qxk2 = ((resp.cerror2 & (0x01 << 8)) != 0);
            error_bits.hxk2 = ((resp.cerror2 & (0x01 << 9)) != 0);

            memcpy(state, &error_bits, sizeof(error_bits));
            break;
        }
    }

    if (i == 2)
    {
        return -3;
    }

    return ret;
}

/*
 * @param state[out]: 感应器反馈
 * @param type: 0-左机头，1-右机头
 */
int fb_get_sensor_state(MH_UDP *pudp, int *state, int type)
{
    short query = type ? CMD_QUERY_FB_IN_CTEST_RC : CMD_QUERY_FB_IN_CTEST;
    QUERY_FB_IN_CTEST_RESP resp;
    int ret = 0;
    int i = 0;
    memset(&resp, 0, sizeof(resp));

    for (i = 0; i < 2; i++)
    {
        ret = cmd_query(pudp, (uchar*)&query, sizeof (query), (RESPONSE*)&resp, sizeof (resp));

        if (ret == 0)
        {
            *state = resp.sensor | (resp.sensor2 << 16);
//            printf("sensor = %x\n", *state);
            break;
        }
    }


    if (i == 2)
    {
        return -3;
    }

    return ret;
}

int fb_get_crg_state(MH_UDP *pudp, CRGSTATE *crgstate, int type)
{
    assert(crgstate != NULL);

    short query = type ? CMD_QUERY_FB_IN_CTEST_RC : CMD_QUERY_FB_IN_CTEST;
    QUERY_FB_IN_CTEST_RESP resp;
    int ret = 0;
    memset(&resp, 0, sizeof(resp));

    ret = cmd_query(pudp, (uchar*)&query, sizeof (query), (RESPONSE*)&resp, sizeof (resp));
    if (ret == 0)
    {
        memcpy(crgstate->yc_state[type], resp.yc_state, 2);
        memcpy(crgstate->cam_state[type], resp.cam_state, 4);
        memcpy(crgstate->sel_state[type], resp.sel_state, 8);
        /* q1, q2, h1, h2, qxk, hxk, q3, q4, h3, h4 */
        memcpy(crgstate->motor_pos[type], resp.st_pos, 10 * sizeof (short));

        /* 三角位置 */
        crgstate->motor_pos[type][10] = resp.cam_pos[0];
        crgstate->motor_pos[type][11] = resp.cam_pos[1];
        crgstate->motor_pos[type][12] = resp.cam_pos[2];
        crgstate->motor_pos[type][13] = resp.cam_pos[3];

        int j = 0;
        for (j = 0; j < 14; j++)
        {
            printf("%d ", crgstate->motor_pos[type][j]);
        }
        printf("\n");
    }

    return ret;
}




